#include "transform/transform.h"

namespace transform {
Eigen::Matrix4d Transform::matrix44()
{
  Eigen::Matrix4d RT;
  double yaw = GetYaw(rotation_);
  RT << cos(yaw), -sin(yaw), 0, translation_.x(),
        sin(yaw),  cos(yaw), 0, translation_.y(),
        0, 0, 1, 0,
        0, 0, 0, 1;
  return RT;
}

void Transform::setTransform(const double &x, const double &y, const double &yaw)
{
  translation_.x() = x;
  translation_.y() = y;
  rotation_ = RollPitchYaw(0.0, 0.0, yaw);
}

void Transform::setTransform(const geometry_msgs::msg::Pose& pose){
  translation_.x() = pose.position.x;
  translation_.y() = pose.position.y;
  //double yaw;
  //yaw = tf::getYaw (pose.orientation);
  //rotation_ = RollPitchYaw (0.0,0.0,yaw);
  //rotation_ = ToEigen(pose.orientation);
  rotation_ =Transform::ToEigen(pose.orientation);
}

Eigen::Quaterniond ToEigen(const geometry_msgs::msg::Quaternion& quaternion) {
  return Eigen::Quaterniond(quaternion.w, quaternion.x, quaternion.y,
                            quaternion.z);
}

Transform ToTransform(const geometry_msgs::msg::Pose& pose) {
  return Transform({pose.position.x, pose.position.y, pose.position.z},
              ToEigen(pose.orientation));
}

Eigen::Quaterniond Transform::RollPitchYaw(const double roll, const double pitch, const double yaw)
{
    const Eigen::AngleAxisd roll_angle(roll, Eigen::Vector3d::UnitX());
    const Eigen::AngleAxisd pitch_angle(pitch, Eigen::Vector3d::UnitY());
    const Eigen::AngleAxisd yaw_angle(yaw, Eigen::Vector3d::UnitZ());
    return yaw_angle * pitch_angle * roll_angle;
}

void Transform::setTransform(Eigen::Matrix4d RT)
{
  double yaw = acos(RT(0, 0));
  if (RT(1, 0) < 0)
    yaw = -yaw;

  setTransform(RT(0,3),RT(1,3),yaw);
}
}